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1. INTRODUCTION 


Many future NASA missions require robotics to assist in the assembly, maintenance 
and servicing of spacecraft. Such scenarios may include one or more multi-linked 
manipulator arms which, because of their lightly damped characteristics, require vibration 
suppression as well as end point tracking in a somewhat uncertain environment. Due to 
the flexibility in the joints/links and the inherent vibration due to the mobility of the 
robotics system, adaptability to the environment and varying inertia is a requirement. 

Several methodologies have been suggested for robot control based upon known tasks 
and environments. Classical proportional-integral-derivative (PID) control has been 
employed in industry for many years. The approach assumes complete knowledge of all 
pertinent system and environmental characteristics. It also requires tuning the PED gains 
to meet some performance specifications. When the system or environmental parameters 
change, the gains must be re-tuned accordingly. Thus, unknown disturbances or 
changing environmental conditions may result in performance degradation. 

To address the issue of uncertainty or time-varying conditions, several adaptive 
control algorithms have been suggested. These include joint-space control [18] and 
global linearization [4] methods in which some nonlinear or discrete matrix polynomial 
equation set must be solved in order to construct the controller. While these methods 
may guarantee stability under certain restrictions, the computation time may limit their 
implementation for multi-linked robotics systems. 

Fuzzy logic control offers an alternative approach in which the structure of the system 
model is not required for control design [25,2]. Fuzzy control algorithms have been 
applied to several process control and automotive systems [8,14,13] in which the time 
constants were somewhat large. The use of fuzzy logic for robotics systems has yielded 
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At this point the unrestricted fuzzy logic controller (Fuzzy-PID) is applied to the 
highly coupled, second order, nonlinear dynamics associated with a three degree-of- 
freedom robotics manipulator. The results of such an application as compared to 
traditional PID control illustrate the performance of Fuzzy-PID. 

Concurrent to this investigation, a coordinated teleoperated mobile manipulator 
system is being designed and fabricated. The system will contain two DR- 106 six-degree 
of freedom manipulators with flexible joints/links supported by a mobile platform. 
Several algorithms such as the fuzzy logic controller developed in this thesis are being 
considered whereby the human operator inputs the desired trajectory and the controller 
tracks the desired trajectory while suppressing vibration and compensating for platform 
motion. Such performance measures are typical for in-space robotics operations. 
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2. BRIEF OVERVIEW OF FUZZY LOGIC CONTROL 


Fuzzy logic control, is quite confusing when initially introduced. However, like 
many concepts in life, once a global understanding is obtained the confusion associated 
with the specifics diminishes. Therefore, before more complicated deliberation on the 
uses of FLC may be developed, it may be in the readers best inters, to consider a brief 
overview of the fuzzy logic control algorithm (FLC). The block diagram of the fuzzy 
logic control algorithm is illustrated in Figure 2.1 



Figure 2 . 1 : Simplified block diagram of fuzzy logic controller 


Fuzzy logic control is a rule based controller. As the term "rule base" indicates, die 
fuzzy logic control algonthm is based on a number of rules which are accessed and 
processed in a specific fashion so as to provide the desired connol input to a system. In 
order to construct such a controller, consideration must be given to the development of 

the rules and how they interact to form the control input. 

Individual rules are constructed using quaUtative terms in conjunction with IF . . . 
then statements. Some examples of common qualitative terms are big. small, large. 


hot, normal, fast, slow, etc. . • 

A linguistic rule used in the process of balancing a stick may read: IF the srick is 
mclined moderately to the left AND ts almost still THEN move the hand to the left 
quickly. In order for this rule to be useful a process must be implemented by which the 
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linguistic terms "moderately", "almost still", and "quickly" are convened into some 
numeric value. Fuzzy set theory does just that. 

Notice how the qualitative linguistic terms are vague in their meaning. This is a 
desired result because it closely resembles how humans think. The process provided by 
fuzzy set theory which enables a linguistic term to take on range of values is called the 
membership function. The membership function is defined over a domain referred to as 
the Universe of Discourse and assumes a value which ranges from 0 to 1. This value is 
referred to as the membership value. In general the membership value is a way to weigh 
how much of a panicular linguistic term is present. 

How does one determine what is "quick"? For example, if the term "quick" is defined 
to be 100 mi/hr. its associated membership value would be 1. Any variations, either 
positive or negative from the speed 100 mi/hr. would result in a membership value of less 
than 1. For example, 90 m/hr. may correspond to a membership value of .9. Therefore 
one may conclude that only 90% of the linguistic term "quick" is present. 

At this point various rules using linguistic terms in conjunction with IF. . .THEN 
statements may be developed. However, because a large number of qualitative linguistic 
terms exist in the human language it is desirable to choose an appropriate number of 
linguistic terms and to define what region they are valid. 

For control applications the linguistic terms tend to read as Large, Medium, Small, 
and Zero. By considering both positive and negative values of the linguistic terms listed 
one has seven distinct qualitative linguistic terms with which to construct rules. (Larger 
Positive, Medium Positive, Small Positive, Zero, Small Negative, Medium Negative, 
Large Negative). These linguistic terms must be defined over a region called the 
Universe of Discourse. It is common practice to define the qualitative terms over a 
quantized Universe of Discourse. By doing so the qualitative linguistic terms may be 
used to describe more than one state in a system. Take for example the rule: IF error is 
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Large Positive AND the rate of error change is Large Positive THEN the control input 
should be Large Negative. Even though the qualitative linguistic term defining error and 
error change is the same (Large Positive) the membership value associated with both of 

the states may differ. 

With the concepts used to develop a rule presented and the foregone conclusion that a 
fuzzy logic controller requires more than one rule to accomplish any reasonable task, a 
brief discussion of the techniques used to process the rule base follows. The rule base is 
simply all of the rules created to perform a particular task. Given the states of one such 
panicular task, it is likely that some of the rules will be appropriate to the conditions 
presented and others will not. The rules that are inappropriate are discarded by the use 
of the Logic Product and the others are combined using the Logic Sum. The result of this 
combination of mles is a weighted area. By finding the Center of Gravity of this area a 
single numeric value results. This value is the final fuzzy inference and is a quantized 
value. Dequantizing this fuzzy inference results in the final control input. 

With this brief overview complete, the reader may proceed further where a more 
detailed descnption of fuzzy logic control and its applications to various dynamic 

systems is presented. 



3. DEVELOPMENT OF 
THE FUZZY LOGIC CONTROL ALGORITHM 
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3.1 MEMBERSHIP FUNCTION 

The basis for Fuzzy Logic Control (FLC) is the membership function, commonly 
referred to as the membership shape. A membership function is defined over a domain 
called the Universe of Discourse (U) and assumes a range from zero to one, referred to as 
the membership value (u). The universe includes all events that can take place in the 
context of a panicular situation. Restated, the universe exists over the boundaries of a 
given situation. 

The membership value (u) describes the probability of an event occurring, given a 
particular universe! 15]. Probability is defined as the ratio of two numbers. The 
numerator represents the events in the universe on which interest is focused, and the 
denominator represents the universe of all possible events. Therefore, the numerator is a 
subset of the denominator. With this in mind, the probability of an event will range from 
zero to one indicating from non-membership to total-membership in the universe. 
Symbolically u(A|B), reads as follows: "the membership value (u) is the probability of 
event A given the universe of events B," where 


u(A|B) = ^ 

Eq.(3.1) 

0<u(A|B)<l 

Eq.(3.2) 


FLC is based on a set of heuristic rules. These rules use qualitative words which 
are defined mathematically in the form of membership functions. To illustrate this 
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The idea of a membership function differs from a binary approach. The dotted box in 
Figure 3. 1 illustrates binary theory where there are only two membership values: zero and 
one. According to this, any person in the range of 5 ft. 4 in. to 6 ft. 2 in. takes on the 
membership value one and is of perfect normal height. Heights less than 5 ft. 4 ins. and 
greater than 6 ft. 2 ins. coirespond to a zero membership value and are thus completely 
non-normal; clearly this is an unnatural situation. 

It has been shown that the exact shape of the membership function is relatively 
arbitrary and may be chosen based on user preference[9]. Figure 3.2 illustrates some 
commonly chosen membership functions. Figure 3.3 illustrates how similar they are by 
superimposing them. 


Bell Shaped Trapezoidal Triangular Sinusoidal 




Figure 3.3: Similarities between some different membership functions 

Due to the complexity of the bell shape (Figure 3.2a) and the piece-wise continuous 
behavior of the trapezoidal shape and triangular shape (Figure 3.2b& 3.2c), the smooth 
continuous and easily calculated sinusoidal shape has been selected as the membership 
function for this study (Figure 3. 2d). 
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3.2 LINGUISTIC RULES 

To develop a fuzzy controller, it is necessary to interpret linguistic rules that are 
based on experience so as to form a control surface that provides output values of the 
controller, corresponding to situations of interest[l 1]. The basis for the linguistic rule is 
the "IF. . .THEN" statement. One linguistic rule or "production rule” describes a portion 
of a particular problem or task in words. The antecedent blocks ("if' phrases) describe 
the states, and the consequent block ("then" phrase) describes how the controller should 
respond to the states. 

For example, asking a first shift operator on an assembly line to describe a single 
portion of his/her task, a typical response may be: 

IF the parts are running "far behind" and they "have been" for a period of time THEN 
I increase the line speed "alot". 

This particular response is based on the operator’s experience and is to be interpreted to 
produce a production rule. However, a complication arises when the same question is 
asked of the second shift line operator. The response may differ in the actual vocabulary 
used, but the premise would remain the same. Therefore, it is necessary to define a 
common or universal set of linguistic terms (common vocabulary) which may be used to 
specifically define the production rule. Refining the operator’s response using specific 
linguistic terms results in a typical fuzzy linguistic rule: 

IF (the error is "large negative") AND (the change in error is "zero") THEN (the 
control input should be "large positive"). 

Notice the change in the antecedents blocks and the consequence block. The terni "parts . 

. . far behind" corresponds to error being large negative, while the term "have been for a 
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period of time" corresponds to change in error being zero. Further, the term "increase . . . 

alot" corresponds to a control input larqe positive. 

In general, in order to develop and interpret this rule, or any other fuzzy linguistic 

rule, the following concepts need to be addressed. 

1. How are error, change in error and control input defined? 

2. How are the qualitative linguistic terms "large positive", "zero", and "large 
negative" defined? 


3 2,1 FRRQR. CHANGE I N ERROR- AND INPUX 

Error is defined as the difference between the process output and the desired output: 


e(k) = e(k)-0,(k) 


Eq.(3.3) 


where 


e(k) = error at time sample k 

0 (k)= position at time sample k 

0 ^ (k) = desired position at time sample k 

The change in error is the difference between the eitor from the current process output 
and the error from the last process output 

ce(k) = e(k)-e{k-l) 


Eq.(3.4) 


where 


All of the examples to follow are maneuvers 
dependent variable is an angle 0 (t) given in 
is the input torque applied to the process. 


ce(k)= change in error at current sample 

e(k) = error at current sample 

e(k-l)= error at previous sample 

of mechanical dynamic systems whose 


either radians or degrees. The control input 



12 


0-9 9 nilAI 1T AT*^P TERM a. 

^ . -3 important to develop a set o 
illustrated in the ,,, 3ame way the .ualitattve 

-"‘"^:“^;;L.en„e.ov«.Keu„ive.eo^ 

linguistic term noroi _ FLC will span a quantized universe or domatn 

6 ft. 2 ins., the lingu.sttc temts or 

^ I.A Thp<;e limits from to 

defined from -6 to +6. Th ^ number. As 

such that the individual membership functions egi^^^^^^^ 

previously mentioned linguistic terms defined over 

linguistic terms. Figure 3.4 illustrate .5^+6 and their respective defining 

• -=-■ - 

functions; large posmve(LP). tnedi 

negauve(SN), medium negative(MN). , trough large-neganve 

The purpose for defining the qualitative 
nn a quantized Universe of Discourse is to ^ 

ehange in error and the condo, input to the ^ ,, 

quantizing the values of eiror. Change in error and 

values -f. to .6 on ihe universe of Disburse (see Fig 



13 






14 



Per example, suppose .he measured error and ca.cula.ed change In e.or .n a 
particular sys.em af.er A/D is 22 degrees and 33 degrees/sec, respeeve y. ^ 
Cnandaadon funedon for er.r, equals one renih of the measured emo., .hen .he ,uana 

error is 2.2. That is, 


quantized error = f ^ 


Eq.(3.5) 


Nonce in Figure 3.5 .ha. if a verucal line is drau,n through .he po.n. 2.2, .. .n er 

.emhership funenons SP and MP. Therefore, .he error is a combinauon of a n«.g 

.he membership function Small PosUive and a weigh, of the membersh.p uncon 
Medium Positive. 

If the quanhzation function for change in enor is defined as 


quantized change in error = x (change in error) 


Eq.(3.6) 
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then the quantized change in error is 2.2. Notice again in Figure 3.5 that this corresponds 
to some membership value of the linguistic term Small Positive and some membership 
value of Medium Positive. Collectively, error and change in error have been shown to 
posses the same quantized value and linguistic values SP and MP while retaining 
different actual values, thereby demonstrating the transcendental usefulness of the 
quantized Universe of Discourse. 

It has been shown that the number of linguistic terms is arbitrary. As the number of 
linguistic terms increases, the resolution of the controller increases as a direct result of the 
induced ability to define each linguistic rule with mote accuracy. In most FLC tt is 
common practice to use only three to five linguistic terms. However, due to the 
complexity associated with the controller for robotics systems, seven linguistic terms are 

employed. 

3.3 RULE BASE 

In order to develop a fuzzy logic controller, a series of mles must be assembled. It is 
the assembly of production mles in which a repenoire of learned problem-solving acdons 
(consequences) is associate with conditions (antecedents), to fonn condition-acdon pairs. 
Once a situation is recognized, the conditions constitute cues or indices for corresponding 
actions. This is how FLC attempts to model the heuristic problem solving approach of 
humansIlS). In the assembly line operator example, one production rule governing the 
case when the error is large negative and change in error is zero was developed. 
However, in order to handle other cases such as the error being medium positive and the 
change in error being large positive, one must develop other rules. Therefore, for each 
particular situation of inteiest, there exists a corresponding production rule. Combination 
of the production rules results in what is referred to as a "rule base". In order to 
the rule base, the concepts of logic produc. logic sum. cemer of gravity, and 
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,uan,.a.io. funcions mu. be developed. An inusa..ive example of an mvened 
pendulum is presented in order to mature these concepts. 


3.3.1 

Figxore 


I n ^jr product 

3.6 illustrates seven rules which are 


commonly used for vibration suppression 


of an invened pendulum. 
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antecedent blocks 



Figure 3.6: Seven rules used for an inverted pendulum 


L0g.c prcduc. .s .he firs, of .he co„cep.s .0 be developed. The phys.cal s.gn.f.ca..ce 
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enmes of .wo se.s, Ma.heroancally, ".he in.ersectio„ of .wo se.s, A n B. co.responds .o 

the AND function and is define by 

u(A AND B) = min(u,(x),Ub(x)) Eq.(3.7) 

Figure 3.6, demonsua.es how .he an.eceden. block or .he quan.ized error and 

quan.ized change in euor are joined by .he AND funcuon, resuUing in a logic produc. 

The dashed verncal lines .n .he quamized =m,r (QE) and quan.ized change .n error 

(QCE) columns represen. a given QE of 3.2 and QCE of .5. Examining .he Hrs. row of 

Figure 3.6, which corresponds .0 Rule 1, one nonces .he quamized enor membersh.p 

value equals 0.8 (u=0.8) and .he change in enor membership value equals 0.9 (u=0.9). 

The minimum of .hese two values is 0.8, therefore u(.8 AND ,9)=.8. The same procedure 

is performed for all of the rules as illustrated in Figure 3.6. 

{min(.8,.9).min(.6,.4).min(.6,0).min(0,.9).min(0,0),nun(0,.4),^^^^^ 

results in the logic product (a set). 

{.8,.4,0,0,0,0,0} 

The logic produc. is used because i. provides the condition in which both enor and 
Change in error are sattsfied. As shown .n rule one (Fig.3.6) the membership value of 
u=0.8 satisfies both conditions; therefore it is nansfened to die consequence block. 


3.3.2 1 OGIC SUa 

The logic sum is an operator such that die connibuuon of each individual rule .s 
-ombined to produce .he f.nal fuzzy inference. Mathematically stated. .. .s .he OR 
funcuon. The union of .wo se.s. A u B, conesponds .o .he OR funcuon and is defined 

u(A OR B)= max(u.(x).Uj(x)) Eq.(3.8) 

The OR funcuon .s applied .o .he consequence block because even if an individual rule's 
influence or conuibution is small, i. should still be reflected in .he resulung connol input. 
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Using the previous values of QE and QCE one can view how the contributions of 
each rule are transferred to the consequence block. 



ANTECEDENT BLOCKS CONSEQUENT BLOCK 



Figures.?: Logic sum 
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The logic sum is the OR function applied to the shaded area in the Control Input 
column shown in Figure 3.7. This shaded area is referred to as the conclusion of the 
furay inference. Figure 3.8 illustrates the result of imposing ail of Uie conmbudons on 

the Universe of Discourse. 

conclusion Of the fuzzy inference is an area and cannot be used directly to 


produce a control command. Therefore, a conversion technique is 
fuzzy inference into a control quantity. The most common 
accomplish this goal is the use of the center of gravity method. 


needed to convert the 
means by which to 



3.3.3 rFNTER OF GRA VITY 

The center c/*rnvny method is the most commonly applied way of combining the 
.ndividual consequences of each rule to get a specific connol quantity that may be sent to 


the process under control. The shaded area 


in 


Figure 3.8 is the final inference produced 


by fuzzy controller. This area, however, cannot 


be used directly to control the output of 
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the system. Therefore, the center of gravity of this area is taken, 
for the center of gravity is 


In general the equation 


QI = yKxU„)/£u„ 


Eq.(3.9) 


where 


QI = Quantized Input 
u„ = membership value 
U„ = Universe of discourse 
Referring to the final inference illustrated m 


Figure 3.8, and using Eq.(3.9) 


QI = (.8 X -4) + (.4 X -2)/(.8+.4) 


Eq.(3.10) 


or 


QI = -3.3 


Eq.(3.11) 


Figure 3.9 illustrates the position of the center of gravity. 



Figure 3.9: Center of gravity method 
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The quantized value of the input QI=-3.3 can now be directly related to a control input 
applied to the system by simply dequantizing it into an applied torque. For example, if 
the dequantizing function is 

Input = ( X (quantized input) Eq.(3. 12) 


then the torque applied to the system 

.=(f)x(-3.3) 


Eq.(3.13) 


is 1=83.33 in-ounces. 

This is the defuzzification operation. The method of defuzzification that employs center 
of gravity is known as the Mamdani method. 

3.4 THF FUZZY CONTROL ALGORITHM 

The overall general fuzzy control algorithm may now be summarized as follows(Fig. 
2.1). First a pair of error and change in error values are measured and calculated 
respectively(Eqs. 3.3 & 3.4). These two states are then convened into quantized error 
and quantized change in error(Eqs. 3.5 & 3.6). These quantized values correspond to 
particular qualitative linguistic terms(Figs. 3.4 & 3.5). The linguistic terms are then 
applied to the antecedent block of the control rules. If both conditions in the antecedent 
block are met then a resulting consequence is registered(Eq. 3.7 & Fig. 3.6). All of the 
individual consequences are then combined by the use of the logic sum(Eq. 3.8 «& Fig. 
3.7). This results in a final fuzzy inference(Fig 3.8). This final inference may then be 
converted into a quantized input by application of the center of gravity method(Fig. 3.9 & 
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Eq. 3.9). The final step is to dequantize the quantized input to the control input to be 
applied to the system(Eq. 3.12). 

The algorithm for FLC just presented is not specific, and it may be applied to a 
multitude of problems. The intent here, however, is to develop a H^C for robotics 
systems. Towards this end, two panicular examples, a horizontal pendulum and a 
vertical pendulum, are supplied. These are given in the Chapters 4. 
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4. ILLUSTRATIVE EXAMPLES 

Befo. applying .he fuzzy logic con.ol algon.hn. developed in Chap.e^^- 
roboncs example (.he coupled nonlinear dynamics of a revolme .hree degree o = 
robot .n this study), some physical insight into the behav.or of the fuzzy log.c contro 
u desirable. This may be obtatned by ftrst applying the fuzzy control aigonthm to 

:.eseco„d.order,linearsystemandcompartng.erespo„setoastep.^^^^^^^^^^^ 

the slightly more complicated dynam.cs of a 
considered. 


4 1 tHF unRl7nNTAL_PEmUM 

H nf length (L) in the horizontal plane with a concentrated 
Consider a massless rod of length (L) 

^ (T\ ciinnlied bv a motor. This system is 
(m) at the endpoint, and an input torque (x) supphed y 


mass 


referred to as a 


Homomal pe^ulum. The equation of motion for the system .s. 


® mL" 


= 0 


Eq.(4.1) 


where 0(t) is the dependent variable defining 


the angular position of the pendulum. 


When 


looking a. the dynamics of this staple system nonce 


(PD) controller is adequate for vibration suppression. 


that a traditional proportional 
This is accomplished by 


derivative 
defining the torque as; 


: = -g(e-eJ-h(e-9d) 


Eq.(4.2) 
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: desired performance specifications. For this 


where g and h are gains chosen to meet ( 
s.ud- .^= -lecnon of .he ga.ns .s based on the Theory oi mural ConlrolWl 

S„b».unng E,.<4.2) into Eq.(4-1) gives the overaU dosed loop systen, dynanucs. 


9 + h(0-ea) + g(^"®<i)“® 


Eq.(4.3) 


Consider a spec.fic fuzzy log.c controller that prov.des .he sanne pertonrrance 
characrerrsrics as .he rradirional PD controller when applied ro .he horrzon.l pendui™. 
For clarif.ca.ron purposes, this fuzzy logic conrroller is referred to as uzzy 
-FPD". I. is nonced in Chapter 3 .ha. rn order to develop a FLC, dre followrng need .0 

defined; 

(1) The number of qualitative linguistic terms used. 

(2) The Universe of Discourse. 

(3) Sign convention on error and change in error. 

(4) The quantization functions. 

(5) The number of rules in the rule base. 



4.1.1 miii^rfr of I INCUISIli 
picrni IRRF. ANP cnNVENTIQM 

Equations 3.3-3 6, and 3. 12 were presenred in such a way rha. they are consrs.en. wrr 
.he Fuzzy-PD connoller now under considerarron. Therefore, .he seven qual.rauve terms, 
1 .he quanuzed Un.verse of Drscourse used ,n the prevrous Chapter w.ll now he 
applred,Flg.3.4 ^ Fig. 3.5). 1. rs mreresring ro nore rha. .he error, change in error and 
to .he system do no. have ro he quantized ro the same seuen qualirauve lin^ 
values. For example, i. may be decided rha. only three iinguisric terms are ne e 
accurate descripdon of error (ex: Poslrive, Zero, Neganve) hu. five llnguisnc terms may 





28 


QE = f 1 X E Eq.(4.4) 

maximum expected error J 

Keeping in mind that the ultimate intent for this thesis study is to develop a FLC to 
control a 3-DOF revolute manipulator, one notices that the DR-106(Fig.5.1) possesses 
physical limitations penaining to the working space. The maximum working space for 
any of the three links is restricted to plus or minus 60 degrees. Therefore, the maximum 
expected error is set to 60 degrees. 

QE = (A)xE E,.(4.5) 

Figure 4.2 shows the error quantization function in Eq.(4.5). Notice that quantized error 
is clipped to either +6 or -6 if the error exceeds the expected limits. 



Figure 4.2: Quantized error as a function of error in degrees 
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4.1. 2. 2 QUANTIZED CHANGE IN ERROR 

The quantization function for change in error is slightly more involved. The change 
in error is the first derivative of error or the slope of the error curve at a particular time. 
If the error changes rapidly with respect to time, the CE(slope) approaches infinity. As 
previously discussed, in order to use CE in the antecedent block it must be quantized. 
How does one quantize change in error values ranging from negative to positive infinity? 

A novel solution to this problem is provided by taking the inverse tangent of the slope 
of the error with respect to the sampling period. This mapping operation provides a 
bounded domain for the change in error between negative 90 and positive 90 degrees. 
With this domain defined on a closed set, it is quite easy to parameterize the change in 
the error as a change in error angle{C^A). 


CEA = tan"’ 


f e(k)-e(k-l) 
I At 


Eq.(4.6) 


where the -90 < CEA < -i-90. Using the bounds of CEA and the same premise developed 
for quantizing error results in quantized change in error angleiQCEA) being defined as: 


QCEA = 




Eq.(4.7) 


The graphical representation of this quantization procedure is illustrated in Figure 4.3. 
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- ^ ppr>LlAMTI7ED INjm 

a dequantization proce . example under 

a ::: ™s »as 

cons.dera„o„, c.e .or,ue vanes response. In general: 

initially selected and adjusted accordi g 


and specifically: 



Eq.(4.9) 


where 
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QI = quantized input 

1= control input to the system (in-ounces) 

This quandzation procedure is illustrated in Figure 4.4. 



Figure 4.4: Torque as a function of quantized control input 



AS a preliminary invesdgaticn leading np to the final Fuzzy-PD controller, seven 
rules were applied to the horizontal pendulum. This resulted in a poor performance 
associated with the controller s lack of ability to handle various wide ranges in initial 


conditions. 


To compensate for this poor performance, an increase in 


the rule base to 36 


rules followed. This control surface also failed due to a lack of robustness. 

Given the fact that there are only seven different values of QE and QCEA, one 
concludes that there ate a total of 49 different possible combinations, corresponding to 49 
distinct rules. Therefore, considering the poor performance of the 36 rules, 
decided to use a control surface fully populated with all 49 different rules. This final 
approach produced acceptable behavior and did not hinder the computational nme 
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associated with the fazay log.c controller. This is due to the fact that, given any pair of 
error and change in error values, only /our of the 49 rules are applied at that riven 

instance. 

The step responses of the horirontal pendulum controlled by both a tradition PD 
controller and the Fuzay-PD controller are tllustrated in Figure 4.5. Both systems possess 
a similar rise times but the Fuzzy-PD controller has considerably less overshoot. This ts 
due to the fact that the controller is based on humanistic rales or the rale base as opposed 


to a mathematical function governed by the damping envelope. 

Since the system is second order, the PD gains could be specifically chosen to 
produce an identical step response to that demonstrated by the Fuzzy-PD controller. 
However, in order to provide an objective test environment, the maximum torque that 
either controller could apply to the system was set to 15 in-ounces. With this in mind 


Fuzzy-PD out performed the traditional PD controller. 
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With a working Fuzzy-PD controller now fully matured, consideration is given to how 
the individual rules can be formulated and how the Fuzzy-PD controller can be tuned. 

4.1.4 POPULATION OF THE RULE BASE 

The rule base was populated by simply allowing the quantized change in error angle 
to be set to zero and looking at how the system behaves as a function of only quantized 
error. For example, if the quantized error is "large positive" and the quantized change in 
error angle is "zero", then the quantized input to the system should be "large negative". 
This rule may be viewed graphically in Figure 4.6 where it is labeled as rule 4. The 
complement of rule 4 may be written as: If quantized error is "large negative" and the 
quantized change in error angle is "zero", then the quantized input to the system should 
be "large positive"(Fig. 4.6, rule 46). The intermediate rules may be found by 
interpolating between these two points. Extruding this slope, both in the positive and 
negative directions of the CEA axis results in an unacceptable controller surface. 
Therefore, the same technique may be applied to obtain the rules along the CEA axis. 
With both the error and change in error axes defined, one may then interpolate across all 
49 different rules. 

Looking at the quantized control input to the system as a function of quantized error 
and quantized change-in-error-angle, one may plot the surface as in Figure 4.6. 


QI = 3(QE,QCEA) 


Eq.(4.10) 
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Interpolating between 


the discrete values produces, a 


smooth fuzzy logic control surface. 


4 1.5 TUNING 

T,e second .cp.c ,o be elaboraced on ^fore deve,opn.e„. of .he more comphcared 
Fuzzy-PID conoouer ,s a .uning scheme. Them are numerous ways ,o tune a uzzy 
Lnler. Phs., .d mos. ob.ons, .he nde base ..se,f may be af.ered.. Second,, .e 
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shape of the membership function may be alteredfsinusoidal, bell, trapezoid, etc. . .). 
Also, the overlap of the membership functions may be altered. And lastly, the quantizing 
schemes or functions may be also changed. Assuming that the rule base, the membership 
function, and the overlap of the membership function have been chosen appropriately, the 
only method of tuning seems to be to change the quantizing functions. 

However, when trying to tune the quantizing functions, a problem arises. This 
problem is associated with the fact that the conuol input is a function of three quanuzed 
terms: the error, change tn error angle, and the input. With no guidance governing the 
relationship between these three quantized values, it may be just as effecnve to 
choose quantizing functions. 

,f two of the three quantization functions were consuatned to certain limits, then there 
would remain only one independent variable to alter. Since, the error is bounded 
(between .60 and -60 degrees) and the change in error angle is bounded (the inverse 
tangent of the slope of the error curve lies between .90 and -90 degrees), the quanuzed 
control input-to-torque relationship remains the only quantization function that may be 
altered. Therefore, it stands to reason that the quantized input to the system ts the only 

choice with which lo tune the fuzzy controller. 

As an off-line approach to tuning, the effect of varying the dequantizanon f 
for Q1 can be examined through a combination of the resulting step response tn 
conjunction with its respective phase portrait. Figure 4.7 shows the system a 
steps in time. From this, the quantized error and quantized CEA can be tabulated as tn 
Table 4 1 A plot of the quantized ermr versus the quantized change in error angle may 
then be obtained; this is the phase portrait. Figute 4.8 illustrates the phase portiatt for the 

Step response illustrated in Figure 4.5 and 4.7. 
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Figure 47 : Pictorial representation of the Fuzzy-PD step response 
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rr ') in the dequantization function (see q. 

By varying the maximum torque (t„„) 

three different time responses and 
. T IS and 80 in-oz, one obtains tnree umt. 

between the values 2.5, , Fimire 4 9(b) shows 

— p.ase po.a..s assoc... w,.H ..=se ..n,e — ^ 

a phase penra.. of an underdamped system when a max.mum q 

o nf thi<i svstem is given in Figure 4.y W- 

applied. The corresponding ..me respon 

Figure 4 9(d) ,s a plot of a phase ponrai. when a max,mum torque o 

.he h— penduium. Tfre assoc, ated time response .s inusuated m T.gure 

n ‘ ,. may he noted tha. .he phase pornait iUusma.ed .n Figure 4.9(d) corresponds .0 a 

4.9(c). I. may 

quick nse ..me wuh a minimal amount of peak oversh 
is the desired curve. 
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Figure 4.9(0 is a phase portrait for a maximum torque value of 80 in. ounces and 
Figure 4.9(e) is its corresponding time response. This particular system is over 
responsive. These three phase ponraits and their corresponding time responses are 
critical to accomplishing tuning of the complicated dynamics associated with the three- 
link revolute manipulator. 



Posiiion (rad) 
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4.1.6 VARYING INERTIA LOAD 

Due to the vary ing task requirements associated with on orbit assemble issues it is of 
imerest to investigaio how the Fuzzy-PD controller compares ro traditional PD control 
when the end-point mass varies. Figure 4.10 illustrates the step response of the 
horizontal pendulum when controlled by both Fuzzy-PD and traditional PD. Figure 
4.10(a) illustrates the response when both controllers arc tuned properly. Figures 4.10(b) 
4.10(d) demonstrates that both controllers are approximately producing the same control 
response for a give situation. This is expected because the fuzzy rules discretely 

approximate the PD control surface. 
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4.1.7 TIME DELAYS 

Another issue of panicular interest is that of time delays. By tuning Fuzzy-PD 
and PD to the same performance characteristics and then introducing time delays a 
comparison of the robustness of each controller may be made. Figures' 4.11 and 4.12 
illustrate the performance degradation associated with increasing the delay time. Figure 
4.11(a) is the step response without any time delays. Figure 4.11(b) demonstrates that 
both controllers behave approximately the same with a time delay of one sample period 
(the sample period used here was .05 sec.). At a time delay of two sampling periods the 
Fuzzy-PD controller becomes marginally stable while the PD controller performs with 
slight indifference(Fig. 4.1 1(c)). Figure 4.12 illustrates that it is not until a five sampling 
period time delay that the PD controller becomes marginally stable. Figure 4.12(c) 
illustrates that as the time delay increase past 4 the system remains marginally stable with 
an increase in amplitude and a slower frequency. 

The results of introducing time delays indicate that Fuzzy-PD control does not 
perform as well as traditional PD control. This is also an expected result due to the fact 
that the traditional PD controller is continuous while the Fuzzy-PD controller possesses a 


discrete number of rules. 
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4.2 THE VERTICAL PENDULUM 

As a second example, consider applying the Fuzzy-PD conrroiler just developed to a 
second-order nonlinear system with a constant system bias. In panicular, consider a 
traditional vertical pendulum. This pendulum possesses two equilibrium points. 
However, it does not operate about either one, thereby maintaining a constant system bias 
due to gravity. The dynamics associated with the pendulum are: 

e + -^sine = t Eq.(4.11) 

L 

As in the first illustration, to implement traditional proportional integral derivative 
control, consider applying the Theory of Natural Control[17], where, 

X = -g(0 - 0 J - h(e - 0, ) - i|(e - e, )ds Eq.(4. 12) 


Linearizing Eq.(4.11) , substituting equation (4.12) into the linearized version of 
Eq.(4.11) and solving for the desired gains g, h, and i results in a PED controller which 
may then be applied to the nonlinear dynamics of Eq.(4.11). 

NOTE: The term Fuzzy-PID represents the fuzzy controller that when applied to a 
system behaves like a tuned traditional PID controller. 

4.2.1 APPLICATION OF FUZZY-PD ON THE — VERTIC . A L 

PENDULUM 

The first attempt at controlling this system using fuzzy logic is to apply the fuzzy 
controller developed for the horizontal pendulum to the dynamics of the vertical 
pendulum. The results of such an approach is illustrated in Figure 4.13 (the curve labeled 
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. .his case is one and the position obtained is 
"course”). The desired position m th 

approxitrately 0.425; therefore, there is a steady-state error. 



Figure 


4.13: Application of the Fuzzy-PL> 


controUer to a tradiliomi ttertical pendulum 


4.2.2 CAEIUBLMEIHQD implemented. 

w fhi^ ‘tteadv-sinle error, a capture met 

I„ order to compensate for thts steady a f„„c,ion every time the 

T,is capture method conststs of redeftning the ,uanttzed error 
error of the system falls within certatn l.nu- ^ 

For example, if the error was ongtna function could be 

minus 60 degrees (coarse), when the error lies e ^ ,io„ function. The 

cedeftnedto have boundsof *30-1 - „ .I,. 

same could be done for a fine quanttzatton function by c ang, 

H,ure 4.13 tllustrates how this method succeeds tn reducing the steady 
never accomplishes the ultimate goal of no steady state error. 
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4.2.3 AN alternative RULE BASE FOR F P]D 

Due to the inability of the Fuzzy -PD controller to deal with the steady-state error, a 
second method using an altered rule base was developed. Figure 4.14 illustrates the new 

rule base. 
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The motivation for defining this new rule base came from observing the phase portraits of 
the time responses illustrated in Figure 4.13. It was noticed tn these phase portraits that 


the ponton of the onginal 49 rule base that compensated for overshoot was producing a 
Steady-State error in the time response. 

Therefore, the rules that compensated for overshoot were eliminated; thus the mle 
base illustrated in Figure 4.14 was assembled. The time response of the venieal 
pendulum to the new rules is illustrated in Figure 4.15. 



f Cti-nv Pin rnntrollcr with an alternative rule base 
Figure 4.15: Step response of Fuzzy-FlU controller w 


;• however once the system reaches its 


It is noticed that this system has a quick nse time, 
desired position, the control input produces no torque. 

ventcal pendulum bach towards it et,u, librium posttion. This cycle repeats itself 
ultimately resulting in a marginally stable system. 

rule base to compensate for system biases fails and another method is needed 


This results in a rapid drop in the 
ition. This cycle repeats itself 
Therefore, the method of altering the 



4.2.4 HYBRID FUZZY-PD AND TRADITIONAL INTEGRAL 
CONTROL 
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In order to compensate for steady-state errors, a fuzzy integral control parameter may 
be augmented to the existing Fuzzy-PD controller. Due to the complexity associated with 
defining the necessary 343 rules, a hybrid controller was designed. This hybrid controller 
uses fuzzy proportional-derivative control (hPD) and classical integral control to produce 
a fuzzy-proponional-integral-derivative controller (FPID). 


X = Xfpd 



(e-0d)ds 


Eq.(4.13) 


The Fuzzy-PD controller handles the vibration suppression as demonstrated earlier and 
the integral term compensates for any system bias. 

The time responses of the vertical pendulum the Fuzzy-PID controller and traditional 

PID control are shown in Figure 4. 16. 



Figure 4.16: Step response of traditional PID and Fuzzy-PID 
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This hybrid Fuzzy-PID controller not only suppresses vibration but also compensates for 
steady-state error. The time response of the Fuzzy-PID controller compared to that of a 
tuned traditional PID controller is hardly distinguishable. With the hybrid Fuzzy-PID 
controller now developed, the approach can be applied to robotics systems. The 
dynamics of the 3-link manipulator are investigated in the next chapter. 



5. APPLICATION OF FUZZY-PID TO A 3-DOF 

MANIPULATOR 
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With the hybrid fuzzy controller developed, its applicability to a robotics system can 
be investigated. A teleroboiic flexible manipulator system is currently being developed at 
the Mars Mission Research Center in order to investigate several control algorithms for 
real-time implementation. A model of one of the robotic manipulator arms is selected for 
preliminary studies of the fuzzy control algorithm. 

Figure 5.1(a) is a drawing illustrating the (DR- 106) three-degree-of-freedom revolute 
manipulator being constructed at North Carolina State University. Figure 5.1(b) 
illustrates the coordinate axis definition. 
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5.1 DYNAMICS 

The nonlinear dynamos for the three-degree-of-freedom revolute manipulator can be 
developed using the Lagrangian approach(22I. This results tn 


‘26 + 8C^+29C^+24CA3 
0 
0 


0 

43 + 24 C 3 
8 + 12C3 


0 

8+12C3 

8 


e. 

02 

03 


-(80,38,3+125X33 + 


(16C,3S,3 + 240 , 8 , 3 ) 0 , (02 + 03 ) + (248,0,3 + 588,0, )0 , 02 

+ 120,5,3 + 29O,8,)0f + 2483 M 3 + 125,0^ - 20g0, - 6gO,3 

-(80,38,3 + 120,5,3)0? - 128,0^ - 6gO,3 


'c, 

^2 


Eq.(5.1) 


Eq.(5.1) may be written more compactly as 


M0 = R+T 


Eq.(5.2) 


where 

M = mass matrix 
0 = acceleration vector 
R = vector containing nonlinear terms 
T = torque vector 

Premultipling Eq.(5.2) by 


9 = M"‘ R+ M"' T 


Eq.( 5.3) 


be written in state space form 


Equation (5.3) may now 
0(t). 


and numerically integrated to find 
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To apply prcpon,caal-inc=gral-derivaave control to the system, the appropriate 
relationship between the torque vector and the positton vector mnst be found. Once again 
the methods developed tn Natural Conmol Theory [17] wtll be ntiliaed. First, make a 
linear approximation by simply dropptng the nonlinear terms in Eq. (5.2). Thts results m 


M6 = T 


Eq.(5.4) 


Secondly, assume 


L 

T = -Ge-H0-ljedt 


Eq(5.5) 


where G, H, and I are control gain matrices. Letnng 

G = gM 
H = hM 
I = iM 

(5.6) into Eq.(5.5) results in the following torque vector: 


Eqs.(5.6) 


Substitution of Equations 


gm,,x(2) + hm„x(3) 

g[m,,x(5) + m„x(8)] f h[m„x(6) ^ m,x(9)j k i[m„x(4) + m,x(7) 

gK>x(5) + "'>.'‘(8)1 + 


Eq.(5.7) 


where x(i) is the i^h state defined as; 


c(l) = j0, -e„ds 

x(2) = 0,-0,a 

x(3) = 0 , 


Eq.(5.8) 
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x(4) = j02-Q2<ids 

x( 5) = 02-02, Eq.(5.9) 

x(6) = 02 

I 

x(7) = j 03 - 03 ,ds 

x(8) = 03-03d Eq.(5.10) 

x(9) = 03 

Figure 5.2 illustrates how the dynamics of the system behaves. With no torque applied 
the links vibrate freely. Figure 5.3 provides a graphical representation of how links 2 and 
3 behave when the links are released from a horizontal position and allowed to move 
freely. Gravity is acti.' g in this system and the two links oscillate about their respective 
equilibrium points. 
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5 2 TPAnmONA I FIIZZY-PIP 

■ The fu.y-P.D hyhnd co„.oUe. applied lo .he venicai pendu.una will eow he app .e 

.0 .his ™a„.paia.o. Each l.nh of .he ™„,pula.oc .s iadependen.ly co„.olled h, 

,, Eachof.he.hreelinksaresubjec.eci.oaum.s.ep 

separate fuzzy logic ccnmrl algonthm. Each of 

forcing funcdon; links I and 2 have posiuve uni. steps and hnk 3 has a nega rv 
..ure 5.4 shows .he response of .he Ers. l.nk using a PP.D hybrid and a — ai P. 


horizontal plane of rotation. 
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Figure 5.4: Step response of link one 

Figure 5.5 illustrates the response of the second link of the manipulator. This graph 
illustrates a slower rise time for the fuzzy-PID controller as compared to the tradition 
PID. However, the fuzzy-PID controller has considerably less overshoot then PID. 
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:r:r— 

of ,hc three links tn terms ol the 3,,„ exhibits hardly any overshoot as 

controller not only produces a qmc 

compared to the traditional PID controller. 



^ a ,h these step maneuvers are Figures 5,7, 5.8 and 5.9, 

The phase portraits associate wi fuizy-PID control provides a 

cespecttvely. Collectively these figures tllustrate that 

. than classical PID control. 





Figure 5.7: Phase portrait of link one 
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6. CONCLUSION AND SUGGESTIONS FOR FUTURE WORK 

This .hesis presented a fnzayPD oo„.oUer made up of 49 rules wirh a s.nusoidal 
„emdersh.p funcuon. TMs general fuzz.-PO conrroUer was r.eu au.men.ed wuH 
^adlrronal Inle^a. conuo. lo produce a fuzzy-P® conooller. T.e fuzzyPID — 
was dren used to control nonlinear robotics models. Fuzzy-PID showed prom.se w 
^.pared to uadluonal P.O due to PH,, re^utremeut of a rrrodel aud the comp.atty 
associated with developing the gains. Because roboncs models, rndudmg space ro 
Urns, contatn uncertarntles, eaact modelW coudollers a. thfncult to tmplement, 

hence the fuzzy approach may be more appealing. 

Pzec.se response charactenst.es us.ng fuzzy conuollers may be ddf.cult, however 

, Cela and Hamam present some stabll.ty Issues associated w.th fuzzy control 

systems. Although exact tuning of such systems may be d.ff.cult, . y 

dta. this process Is less difficult than PID gain tuning. Phase pormuts prov.de 

off-line tuning approach. ande 

The use of a fully populated ^uanuzed error and quantized change tn error angl 
rule base prov.des a more effect.ve conuoller while no. hindenng the processtng ume o 
: oilier. Funher, ut.l.zat.on of the .nverse target funedon provtdes a novel means 

with which .0 bound the quantized change in error angle term. 

several .ssues may be cons.dered as future acdvrtles, .n order to ex. nd . 

roohnlque to space robot.es systems and .n particular, the ground testbed 
oonsmuced a. North Carolina State University. F.rst, fuzzy log.c conuo, may 
oonsmucted .n such a fashion so as to tune a madltlonal Pfi> conuoller. Th.s appro 
.ould result tn an adapttve controller that could compensate for vary.ng .nertta loads. 
,i„e delays; ultimately resuldng in an .deal candidate for space appl.cat.ons. 
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Secondly, other calibration techniques including on-line methods are being 
considered. These methods include requantizing other variables besides the control input. 
Such extensions will be investigated and compared to other control methods being 
developed and implemented for space robotics systems. 
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8. APPENDICES 


8.1 pn^ p^oTinMAI -INTERRAI - nERIVATiYECQtilBPLmJ. 

nr>F manip ulator 

PRCX3RAM PID ON RO®?Lcccccccccccccccccot^^ 
cccccccccrerecaxccKcc^^^^^^^^^ control to the nonlinear dynamics of c 

a three link microbot. 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


AUTHOR: Robert J. Stanley II 
DATE: 8/5/92 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


Torque applied to 

Td#‘ The desired angle of each unK 

submunne is calied 

DT: Time interval delta T in rad/sec 
TIME: Independent variable 

X(I)' Dependent variable ^ 

Lcccccccccc^SLrcc'c&cccccc 

„ toRMAT(lX,F6.2,lX,F10.4,lX.F10.4.1X,Fi0.4, 


NEQ=9 

dt=.oido 

nstep=iooo 

TIME=0.0D0 

X(1)=0.0 

X(2)=-1.0 

X(3)=0.0 

X(4)=0.0 

X(5)=-1.0 

X(6)=0.0 

X(7)=0.0 

X(8)=1.0 

X(9)=0.0 
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Tdl=1.0 

Td2=1.0 

Td3=-l.O 


CALL T0RQUE(X,T1 T2 X(5)+Td2,T2,X(8)+Td3,T3 

CALL TORQJ^C^TL'n^^^ T1 Y(5)+Td2,T2,X(8)+Td3,T3 

S9:»xaHTdi>(5H^^ 

100 CONTINUE 
CLOSE(8) 


r:xrr\ 


SUBROUTINE T0RQUE(^XT1T2TO^ 
control. 


c 
c 
c 
c 
c 
c 
c 
c 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


AUTHOR: Roben J. Stanley II 
DATE: 8/5/92 

VARIABLES: 

X: State vector 

Tdl* Theta Desired One 


1 nuia 

Td2: Theta Desired Two 
Td3: Theta Desired Three 

Tl- Torque applied to link one 
T2: Torque applied to link Two 

T3; Torque applied to Enk Thre 
M##; The respective elements ot tn 
H#; The derivative gain 

G#; The portional gain 

I#; The integral gain 
alpha* The exponential decay rate 

LccccccccJ— 

/mYCOMM/ Tdl,Td2,Td3 


c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


ALPHA=2.46051702 

BETA=3.14 



G1=3*ALPHA**2+BETA**2 

H1=3*ALPHA 

11 = 0.0 

G2=3*ALPHA**2+BETA**2 
H2=3* ALPHA 

12=ALPHA*(ALPHA**2+BETA**2) 

G3=3*ALPHA**2+BETA**2 

H3=3*ALPHA 

13=ALPHA*(ALPHA**2+BETA**2) 


C23=COS(X(5)+Td2+X(8)+Td3) 

S23=SIN(X(5)+Td2+X(8)+Td3) 

C2=COS(X(5)+Td2) 

C3=COS(X(8)+Td3) 

S2=SIN(X(5)+Td2) 

S3=SIN(X(8)+Td3) 

C23S23=C23*S23 

C2S23=C2*S23 

S2C23=S2*C23 

S2C2=S2*C2 

C7Si2=C2*S2 


M11=26.+8.*C23**2+29.*C2**2+24.*C2*C23 

M22=43.+24.*C3 

M23=8.+ 12.*C3 

M33=8. 


■Ti— 1 1 1 1 *X^2^+H 1 *M1 1 *X(3)+11*M1 1*X(1)) T2 

T*(G24S2^X(5HM23*X(8))+H2*(M2 

4-4-T'^*rM23*Xf4)+M33*X(7))) 


return 

END 


SUBROUTINE R1GHT(R1,R2,R3,X,M11,M22,M33,M23,DET) 

cccccccccrcoxwccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C PURPOSE: To calculate the respective mass matnx entnes and 

the respective nonlinear contributions Rff- ^ 

AUTHOR: Robert J. Stanley II ^ 

DATE: 8/5/92 ^ 

VARIABLES: . . ^ 

R#: The nonlinear terms of link # respectively ^ 

F: State space ^ 

DFT' The determinate of the mass matrix divided by Ml 1 c 

Jcccccccc™cS;™^ 
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CmitON /MYCOMM/Tdl.Tdi^^^ 

C23ioS(X(5)+Td2+X(8)+TO 

S23=SIN(X(5)+Td2+X(8)+Td3) 

C2=COS(X(5)+Td2) 

C3=COS(X(8)+Td3) 

S2=SIN(X(5)+Td2) 

S3=SIN(X(8)+Td3) 

C23S23=C23*S23 

C2S23=C2*S23 

S2C23=S2*C23 

S2C2=S2*C2 

C2S2=C2*S2 

M11=26.+8.*C23**2+29.*C2**2+24.*C2*C23 

M22=43.+24.*C3 
M23=8.+ 12.*C3 

DET=M22* M3 3 - M23 * M23 

R1^6^^3^*-S'^*Ss23rX(3)nW^ 

*12023+58 •S2C2)*X(3)*^ 

r3=.(8.*C23S23+12*C2S23)*X(3)**2-12.*S3*X(6)**2-6*G*C23+T3 

return 

END 


SUBROUTINE STATE(F.X .T1M E) ,,..,,, ,,,ccccccccccccc 

cccccccccccccccccccrccccccccccc^ dynamic system. 

C PURPOSE: To compute the present state oi uic y ^ 

Q 

c AUTHOR: Robert J. Stanley II c 

c ^ 

c DATE: 8/5/92 c 

c ^ 

c VARIABLES: , ^ ^ . c 

^ccccccccccccc"c“^^^^^^^^ 

COMMON /MYCOMM/ Tdl,Td2,Td3 



TIME=TIME*1.0 


CALL RIGHT(R1,R2,R3,X,M1 1,M22,M33,M23,DET) 

F(1)=X(2) 

F(2)=X(3) 

F(3)=R1/M11 

F(4)=X(5) 

F(6)?R2*M33/DET)-(R3*M23/DET) 

F(7)=X(8) 

SoL^R2*M23/DET)+(R3*M22/DET) 


return 

END 


SUBROUTINE RUNGA(X DT NEQ^^^ 

u“ Ku..a roudne to compute the next sme ^ 

c vector c 

c AUTHOR: Roben J. Stanley II c 

c ^ 

c DATE: 8/5/92 c 

c ^ 

c VARIABLES: c 

'ecccccccccca^rcrcccccccececccccecccc.^^^^^^ 

REAL*8X(10),Y(10),F(10),DT.TIME.Gl(10),G2(10).G3(10),G4aO) 

TKTTPr.FR I.bfeo 


1 


2 


3 


4 


DO 1 1=1, NEQ 
Y(I)=X(I) 

CALL STATE(F,Y,TIME) 
DO 2 1=1, NEQ 
G1(I)=DT*F(1) 


IME=TIME+DT/2.0D0 

0 3 1=1, NEQ 

Y(I)=X(I)+G1(I)/2.0D0 

;ALL STATE(F,Y,TIME) 
)0 4 1=1, NEQ 
G2(1)=DT*F(I) 
Y(I)=X(I)+G2(I)/2.0D0 

:all state(F,y,time) 
)0 5 1=1, NEQ 
G3(I)=DT*F(I) 
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5 Y(I)=X(I)+G3(I) 

TIME=TIME+DT/2.0D0 
CALL STATE(F,Y,TIME) 

D0 6I=1,NEQ 

6 G4(I)=DT*F(I) 

7 °° XaiwWG 1 a)+2.0D0*(G2(I)+G3(I))+G4(I))/6.0D0 

eieturn 

END 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


c 

c 

c 

c 

c 

c 

c 

c 


7 FI I77Y L^nir r.nNTR Q i DN 3 -DOF MANIPULAT OR 

AUTHOR: Robert J. Stanley II 
DATE: 8/4/92 

VARIABLES. ^ <->f the nresent Runge-Kutta iteration c 

COUNT: Holds the value of the present ivuhb ^ 

TORQUE: The input to the system 
INERTIA: The inertia of the system 
Wn: The natural frequency of the g^s^em 

n.nga.ku,. subrounne is oaUed 

DT: Time interval delta T in rad/sec 
TIME: Independent variable 
X(I): Dependent variable 

I F(I):’ State equations ^^^^^^_„„ccccccccccccccccccccccccccccc 

lOTEGER Td3 

COMMON /MYCradW T1 ,T 2 ^. ‘ 

8iS(12?SjHrU 

OPEN(9,FM=;fp.drl ^ 

"■J/,RM\TaX.F6.2,lX,F10.4,lX,F10.4.1X,F10.4) 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 




ALPHA=.46051702 

BETA=3.14 


12-ALPHAnALPHA* n+BCT A**2) 

I3=ALPHA*(ALPHA**2+BETA 2) 

Ll=l 


L2=2 
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L3=3 

NE0=9 

DT=01D0 

NSTEP=1000 

TIME=0.0D0 

X(1)=0.0 

X(2)=-1.0 

X(3)=0.0 

X(4)=0.0 

X(5)=-1.0 

X(6)=0.0 

X(7)=0.0 

X(8)=1.0 

X(9)=0.0 

Tdl=1.0 

Td2=1.0 

Td3=-1.0 

TRIG 1=0 

TRIG2=0 

L0GIC(X,T1,TRIG1,L1) 

RjKY L0GIC(X,T2,TR1G2 U 
CALL FUZZY_L0GIC(X,T3,TRIG3,L3) 

TRIG1=1 

TRIG2=1 

™=CoVx(5)+Td2+X(8)+Td3) 

C2=COS(X(5)+Td2) 

M22=43.+24.*C3 

M23=8.+12.*C3 

c A,„.eSn,ue produced by .he Fu.y coudoUer w..h .ha. of c .he dad..io„af 
Integral feedback 

TLT742*(M22*X(4)+M23*Xa 

T3=T3-I3*(M23*X(4)+M33*X(7)) 

c T1=0.0 

c T2=0.0 

C^L FU^Y"LOGIC(X,T2,TR G2|2 
CALL FUZZY_L0GIC(X,T3,TRIG3,L3) 
C23=COS(X(5)+Td2+X(8)+Td3) 

C2=COS(X(5)+Td2) 

C3=COS(X(8)+Td3) 
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Mll-26.+8.*a3**2+29*C2**2+24.*C2*C23 

M22=43.+24.*C3 

M23=8.+12.*C3 

. ,rthv the Fuzzy controller with ihat of c the ffadinonal 
c Agutnent the torque produced by the Fuzzy 
Integral feedback 

SSll2nM22*X(4)+^3*Xg)^^ 

T3=T3-I3*(M23*X(4)+M33 X(7)) 

„ T1=0.0 

r T2=0.0 

c T3=0.0 vn^4-Tdl T1 JC(5)+Td2,T2^(8)+Td3.T3 

100 CONTINUE 
CLOSE(8) 

CLOSEC9) 

CLOSE(IO) 

CLOSE(ll) 

END 

SUBROUTINE 

„ecccccc^c™^^^^^ c 

" Yh?Spective nonlinear contnbuuons R#. 

c c 

^ AUTHOR: Robert J. Stanley II c 

c c 

I DATE: 8/5/92 c 

^ c 

" '^'^R‘^®^t^nonlinear terms of link# respectively ’ = 

^ F: State space c 

„ G: Gravity . , matrix divided by Ml 1 

UccccccccSScS;=rcc^c?ccccccccccccc^^^^^ 

rEAL*8 S3,C23S23 C2S23 S2C^.b^^ 

COMMON /MYCOMM/ T1 ,T2, 1 3, lai, i 

P93=COS(X(5)+Ed2+X(8)+Td3) 

S23=SIN(X(5)+Td2+X(8)+Td3) 

C2=COS(X(5)+Td2) 

C3=COS(X(8)+Td3) 

S2=SIN(X(5)+Td2) 

S3=SIN(X(8)+Td3) 

C23S23=C23*S23 

C2S23=C2*S23 


S2C23=S2*C23 

S2C2=S2*C2 

C2S2=C2*S2 


M11=26.+8.*C23**2+29.*C2**2+24.*C2*C23 

M22=43.+24.*C3 

M23=8.+ 12.*C3 

M33=8. 

DET=M22*M33-M23*M23 


R1=(16.*C23S23+24.*C2S23)*X(3)*(X(6)+X(9)) 

-h-(24.*S2C23+58.*S2C2)*X(3)*X(6)+T1 


R2=-(8 *C23S23+12.*S2C23+12.*C2S23+29.*C2S2)*X(3)**2 

++24 *S3*xT6)*X(9)+12.*S3*X(9)**^ 


R3=-(8.*C23S23+12.*C2S23)*X(3)**2-12.*S3*X(6)**2-6.*G*C23+T3 

RETURN 

END 


SUBROUTINE STATE(F,X,TIME) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c PURPOSE: Define the dynamics in a state space form tor use in c 

c Runge Kutta Subroutine. ^ 

C p 

c AUTHOR; Roben J. Stanley II ^ 

c c 

ccccccccScSccccSccccccccccccccccccccccccccccccccccccccccccccccccccccc 

real* 8 X( 1 0),F( 1 0),TIME,Td 1 ,Td2,Td3,Tl ,T2,T3 

REAL*8 R1,R2,R3,M1 1,M22,M33,M23,DET 
COMMON /MYCOMM/ Tl,T2,T3,TdLTd2,Td3 

TIME=TIME*1.0 

CALL RIGHT(R1,R2,R3,X,M1 1,M22,M33,M23,DET) 

F(1)=X(2) 

F(2)=X(3) 

F(3)=R1/M11 

F(4)=X(5) 

F(5)=X(6) 

F(6)=(R2*M33/DET)-(R3*M23/DET) 

F(7)=X(8) 

F(8)=X(9) 

F(9)=-(R2*M23/DET)+(R3*M22/DET) 


RETURN 

END 
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SUBROUTINE RUNGACXJT 

.eccccccccc.™^^^^ solve .he c 

" '^‘^^a.e =qua..ons given is subrounne STATE. 

^ c 

c AUTHOR: Roben J. Stanley II c 

REAL*8X(10),Y(10),F(10).DT,TIME,GU10),G2(IO).G3(10),G4aO) 

INTEGER I,NEQ 

DO 1 1=1, NEQ 

1 Y(I)=X(I) 

CALL STATE(F,Y ,TIME) 

DO 2 1=1, NEQ 

2 G1(I)=DT*F(I) 

TIME=TIME+DT/2 .ODO 

3 1 (D/2.0D0 

CALL STATE(F,Y ,TIME) 

D0 4I=1,NEQ 

G2(I)=DT*F(I) 

4 Y(I)=X(D+<j2(I)/2.0D0 

CALL STATE(F,Y,T1ME) 

DO 5 1=1, NEQ 

G3(I)=DT*F(1) 

5 Y(I)=X(I)+G3(I) 

TIME=T1ME+DT/2.0D0 
™lI STATE(F,Y,TIME) 

DO 6 1=1, NEQ 
5 G4(I)=DT*F(I) 


DOy='j'^,Q(Q,(,).,2.0D0nG2(IHG3(I))-sO4^ 

return 

END 


END 


SUBROUTINE E^ZZY LOGIW 

ecccccccccocc^c^c^^^^^^^^ a torque required to c 
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c 

c 

c 

c 

c 

c 

c 

c 

c 


c 

c 


c 

c 

c 

c 


drive the error to zero using Fuzzy Logic Control. 

AUTHOR: Robert J. Stanley II 

DATE: 8/4/92 

VARIABLES: 

E: Error 

CE: Change in Error 
CEA: Change is Error Angle 

LASTE: The last error 

SET_PT: Set point desired 

PI: 3 T 14 

QE: Quantizied value of the ®iror 
QEC’ Quantizied value of the Error Chang 
u: Membership function value 
UU; Universe of discourse value 
NUM- NUMerator of the input value 

INPUT: The quantizied input to the plant 
TORQUE: The actual input to the plant 
N: Number of rules 

gSdSs which grid is being urilized 

gKi!a°S accessed 

ELP' Linguistic value Error Large Positive 
EMP: Linguistic vfne Error Medium 
ESP' Linguistic value Error Small Pos 

Im- UnluiS vtut tor fnn”u Negauve 

IK5ivS«r;.. 

CESP' Linguistic value Change Error 

LYngSc^le Large Neganve 


c 

c 


c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


Lcccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

REAL»8 ECE.LASTC^HjQE,QEC^m 

11^*4 

INTEGER N,I,TRIGGER,GRID,LINK 
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16 


FORMAT(1X,F10.4,1X,F10.4,1X,F10.4,1X,F10.4,1X4=.0.4) 


elp=.false. 

emp=false. 

ESP=.FALSE. 

eze=.false. 

ESN=.FALSE. 

emn=.false. 

ELN=.FALSE. 

CELP=.FALSE. 

CEMP= .FALSE. 

CESP=.FALSE. 

CEZE=.FALSE. 

CESN=.FALSE. 

CEMN=.FALSE. 

CELN=.FALSE. 

fine=.false. 

MEDIUM=.FALSE. 

COARSE=.FALSE. 


N=49 

PI=3.14 

IF(LINK.EQ.1)THEN 

^(tSgGER.EQ.O) then 
CEA=0.0 

^^^(2EA=ATAN2(E-LASTE1,.01) 

END IF 
LASTE1=E 
EMAX=60.0 
TOR_MAX=500.0 
END IF 

IF (LINK.EQ.2) THEN 

(TRIGGER.EQ.O) THEN 
CEA=0.0 

CEA=ATAN2(E-LASTE2,.01) 

END IF 
LASTE2=E 
EMAX=60.0 
TOR_MAX=500.0 
END IF 

(link.eq.3) then 

IF (TRIGGER.EQ.O) THEN 
CEA=0.0 



ELSE 

CEA=ATAN2(E-LASTE3,.01) 

END IF 
LASTE3=E 
EMAX=60.0 
TOR_MAX=150.0 
END IF 

c Change error and change in error from radians todegmes. E=(180/3,14)*E 

c DererrrnScMnS^^^ is .0 be used and fmd .he c corresponding 
,„an.iaiedvalues^^=.^ 

C fine=.true. 

c QE=E*(6/25.0) 

c QECA=CEA*(6/90.) 

? n= ((E.LT.33.0).AND.(E.GT.-33.0)) then 

^ medium=.true. 

QE=E*(6/33.) 

QECA=CEA*(6/90.) 

ELSE 

COARSE=.TRUE. 

QE=E*(6/60.) 

QECA=CEA*(6/90.) 

END IF 
END IF 

COARSE=.TRUE. 

QE=E*(6/EMAX) 

OECA=CEA*(6/90.) 


c 

c 

c 

c 

c 


C Deterimine which grid is being used 
IF (COARSE) THEN 
GRID=1 
END IF 

IF (MEDIUM) THEN 
GRID=2 
END IF 

IF (FINE) THEN 
GRID=3 
ENDIF 


c With the Quantizied Error determine which c 
linguistic values are applicable. 

IF (QE.GE.6.0) THEN 
QE=6.0 
elp=.true. 

ff^QDGE.4.0).AND.(QE.LT.6.0)) THEN ELP=.TRUE. 

emp=.true. 

END IF 
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IF ,(QE.GE.2.0).AND.(QE.LE.4.0)) THEN EMP=TRUE. 
ESP=.TRUE. 

^^QiGE.0.0).AND.(QE.LE.2.0» THEN ESP=.TUUE, 
EZE=.TRUE. 

«QE.GE.-2.0).AND.(QE.LE.0.0)) then 

eze=.true. 

ESN=.TRUE. 

^'ff((QE.GE.-4.0).AND,(QE.LE.-2.0))THEN 

ESN=.TRUE. 

EMN=.TRUE. 

®®(«E.GE.-6.0).AND.(QE.LE.-4.0))THEN 

EMN = .TRUE . 
eln=.true. 

ff%tLE.-6.0) THEN 
QE=-6.0 
ELN=.TRUE. 

end if 

- A Prrnr Change determine which 
c With the Quantizied Error b 

QECA=6.0 

CELP=.TRUF. 

|ToicA.GE.4.0).AND.(QECA.LT.6.0))THEN 

CELP=.TRUE. 

CEMP=TRUE. 

ITqECA.GE.2.0,.AND.(QECA.LE.4.0,)THEN 

CEMP=TRUE. 

CESP=.TRUE. 

ff«^fcA.GE.0.0).AND.(QECA.LE.2.0))THEN 

CESP=.TRUE. 

CEZE=.TRUE. 

|^KAGE.-2.0).AND.(QECA.LE,0.0»THEN 

ceze=.true. 

CESN=.TRUE. 

,#[SeCA.GE.-4.0).AND.(QECA.LE..2.0,)THEN 

' CESN=.TRUE. 

CEMN=.TRUE. 

:f"®ECA.GE..6.0,.AND.(QECA,LE..4.0))THEN 

CEMN=.TRUE. 
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CELN=.TRUE. 

ff^KA.LE.-6.0) THEN 
QECA-6.0 
CELN=.TRUE. 

end if 


DO 250 1=1, N 

u(I)=0.0 

UU(1)=0.0 

roNTINUE 


H the rhanee in Error is c Large Negative 
C Rule one if Error is Large Pcsilive and the Chang 

.hen conmbunon js too 

^ Ye=SIN(PI/4*(QE-4;0)) 

Yec=SIN(PI/4*(QECA+8.0)) 

u(l)=MIN(Ye,Yec) 

UU(1)=0.0 

END ff ,he Change in Error is e Med.um 

c Rule two if Error is Large ros ^ 

u(2)=MIN(Ye,Yec) 

.R„,erh&"-gePos.nveand.heChangein Error rscSroallNega^^^ 

u(3)=MIN(Ye,Yec) 

UU(3)=-4.0 

eRulefol^E^-sEargePosidve and the Change in Error isctoo .hen 

eonrrihurion^^^^^^^ 

u(4)=MlN(Ye,Yec) 

UU(4)=-6.0 

c Rule rwe^E^r is Large Posinve and .he Change ,n Error is e Sroall Posrnve 

.Hencon.nb.nron is 

Ye=S IN (PI/4* (QE-4.0)) 
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Yec=SIN(PI/4*(QECA)) 

u(5)=MIN(Ye.Yec) 

UU(5)=-6.0 . . 

c R„,e JTeZ. . Large PosiUve and .de Cnar.ge ,n Error ,s c Media. Posmve 

u(6)=MIN(Ye,Yec) 

UU(6)=-6.0 , . 

, R„.e se^&or rs Large Posurve and .he Change in Error .s c Large Pos.hve 

u(7)=MIN(Ye,Yec) 

UU(7)=-6-0 

oan,eeifh^pf.or.s"^ 

Yec=SIN(PI/4*(QECA+8.0)) 

u(8)=MIN(Ye,Yec) 

UU(8)=2.0 

.an.e„.L^E".or.sM— 

Negative Aen^onmta Z 

Yec=SlN(Pl/4*(QECA+6.0)) 

u(9)=MIN(Ye,Yec) 

UU(9)=0.0 

e Rn.e .e^^E^r^r .s Med.n. Pos..,ve and .he Change .n Error .s c S.aU Negatrve 

u(lO)=MIN(Ye,Yec) 

UU(l0)=-2.0 

. Rn,e eS .^Error is Med.nn. Posirtve and .he Change in Error ,s c Zero d.en 

oonmbu..on|Med.jnnNe^^^^^^ 

‘ “=Sm(PU4*(QE-2W) 

Yec=SIN(PI/4*(QECA+2.0)) 

u(ll)=MlN(Ye,Yec) 

UU(ll)=-4.0 
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, . Med.u. Pos.nve and .He Change .n Erxor .s c S^aH Posidve 

Ye=SIN(PV4nQE-^0)) 

Yec=SIN(PI/4*(QECA)) 

u(12)=MIN(Ye,Yec) 

UU(12)=-6.0 . . 

..„,e.HffiB.on.sMedin.Po.^^^^ 

then WxS then 

Ye=SIN(Pl/4*(QE-2^0)) 

Yec=SIN(PI/4*(QECA-2.0)) 

u(l3)=MIN(Ye,Yec) 

UU(13)=-6.0 . . 

,au,efo»E.on.sMed.nn,Posu.veandd,eCHange.E.on.scLange^ 

.Hen con— 

"^%e=SIN(PI/4*CQE-2W) 

Yec=SIN(PI/4*(QECA-4.0)) 

u( 14 )=MIN(Ye,Yec) 

UU(14)=-6.0 

e an>e «^e^i?E.4or .s S^al, PosUWe and .He Change in Ereon .s c Large Nega..ve 

u(15)=MlN(Ye,Yec) 

UU(15)=4.0 

ean,es.aTnirE.rorisS™^ 

then contribution is 

TP (ESP.AND.CEMN) THblN 
Ye=SIN(PI/4*(QE)) ^ 

Yec=SIN(Pl/4*(QECA+6.0)) 

u(l6)=MIN(Ye,Yec) 

UU(16)=2.0 

f F or is Small Posinve and the Change in Error is c Small Neganve 
C Rule seventeen if Error is bmaii t^osiuvc 

,Henconmbu|on^ra^SN)THEN 

Ye=SIN(PI/4*(QE)) 

Yec=SIN(PI/4*(QECA+4.0)) 

u(17)=MIN(Ye,Yec) 

UU(17)=-2.0 

c anle e.fh^er.f Error ,s Small Pos.nve and .He Change .n Error is c Zero .Hen 
conmbu.l>n is Small Neganve. 
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IF (ESP AND.CEZE) THEN 
Ye=SIN(PI/4*(QE)) 

Yec=SIN(PI/4*(QECA+2.0)> 

u(l8)=MIN(Ye,Yec) 

UU(l8)=-2.0 . . 

"'f E^r is small Positive and the Change in Emit is c Small Postuve to 
c Rule nineteen if Error is itndu r 

u(l9)=MIN(Ye,Yec) 

UU(l9)=-4.0 . . 

e Rnle iTe- ts Small Ptove and the Change In Emon ts c Medmm osmve 

to eonmbunon ^^8=gNeg 

u(20)=MIN(Ye,Yec) 

irU(20')=-6.0 . . 

'L if Error is Small Postuve and ihe Change In Error is c Large Postuve to 
C Rule twenty one it Error is omai 
contribution is Earge Negative. 

IF (ESP.AND.CELP) IrltiN 
Ye=SIN(PI/4*(QE)) 

Yec=S IN (PI/4* (QEC A-4.0)) 
u(21)=MIN(Ye,Yec) 

UU(21)=-6.0 

^ if prror is Zero and the Change in Error is c Large 
c Rule twenty two Positive. 

Neeative then contnbution is 

^ TF (EZE AND.CELN) THEN 
Y==SIN(PU4*(QE+2.0)) 

Yec=SIN(PI/4*(QECA+8.0)) 

u(22)=MIN(Ye,Yec) 

UU(22)=6.0 

? f Error is Zero and the Change in Error is c Medium 

Yec=SIN(PI/4*(QECA+6.0)) 

u( 23 )=MIN(Ye,Yec) 

UU(23)=4.0 

c.nleJe!SLtfEmor.s^^^^^ 
then contribution is 

Te=SIN(PI/4nQE+2.0)) 

Yec=SIN(PI/4*(QECA+4.0)) 
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u(24)=MIN(Ye,Yec) 

UU(24)=2.0 

if Error is Zero and the Change in Error ,s c Zero ihen 
c Rule twenty five it brror is ^ 

contribution THEN 

IF (EZE.AND.CEZb) 

^ Ye=SIN(PI/4*(QE+2,.0)) 

Yec=S IN (PI/4* (QEC A+2.0)) 
u(25)=MIN(Ye,Yec) 

UU(25)=0.0 

c Rule ™.x rf Error ,s Zero and .he Change in Error is c Snrall Posrnve 

^ Ye=SIN(PI/4*(QE+2.0)) 

Yec=SlN(PE4*(QECA)) 

u(26)=MIN(Ye,Yec) 

UU(26)=-2.0 

END IF ^ Change in Error is c Medium 

u(27)=MIN(Ye,Yec) 

UU(27)=-4.0 

U Rule T^y^rgh. if Error is Zero and .he Change in Error rs c Large Pos...ve 

Ye=SIN(PE4*(QE+2.0)) 

Yec=SIN(PI/4*(QECA-4.0)) 

u(28)=MIN(Ye,Yec) 

UU(28)=-6.0 

END IF . ci^^iiNeeaave and the Change in c Error IS 

C Rule Twenry nine 

,3rgeNe^anve.h.— 

u(29)=MIN(Ye,Yec) 

UU(29)=6.0 

(ESN.AND.CEMN) TmN 

Ye=SIN(PE4*(QE+4.0)) 

Yec=SIN(PE4*(QECA+6.0)) 

u(30)=MIN(Ye,Yec) 

UU(30)=6.0 

END IF 
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p r Small Negative and the Change in c Error is Small 

-SiSS- 

Ye=SlN(PI/4*(QE+4.0)) 

Yec=SIN(PI/4*(QECA+4.0)) 

u(31)=MIN(Ye,Yec) 

UU(31)=4.0 

e R„,e if E.O. .s S^ai. NegaUve and .he Change in c Ern,. .s Zero 

U,enconmb«^on^^^^^^ 

u(32)=MIN(Ye,Yec) 

UU(32)=2.0 , 

„ , T^r.hree.f Error .s small Negacive and the Change in c Error .sSnra 

Ye=SIN(Pl/4*(QE+4.0)) 

Yec=SIN(PI/4*(QECA)) 

u(33)=MIN(Ye,Yec) 

UU(33)=2.0 

end IF . Onfall Negative and the Change in c Error IS 

‘^‘Ye=SIN(PW*(QE+4.TO 

Yec=SlN(Py4*(QECA-2.0)) 

u(34)=MIN(Ye,Yec) 

UU(34)=-2.0 

END IF Negative and the Change in c Error is Larg 

Ye=SIN(PI/4*(QE+4.0)) 

Yec=SIN(PI/4*(QECA-4.0)) 

u(35)=MIN(Ye,Yec) 

UU(35)=-4.0 

END^F . ,aru.m Negative and the Change in c Error is 

Ye=SIN(PI/4nQE+6.0)) 

Yec=SIN(PI/4*(QECA+8.0)) 

u(36)=MIN(Ye,Yec) 

UU(36)=6.0 

c Rule Th" ^seven if Error "" ' 

" ^^EtNANDTEMTlTON 
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:. 0 )) 


Ye=SlN(PI/4*(QE+6.0)) 

Yec=SlN(PI/4*(QECA+6.0)) 

u(37)=MIN(Ye,Yec) 

UU(37)=6.0 . 

xJ®® ‘Lt if ETor .s Medium Negacive and the Change in c Emor is Smal 
c Rule Thirty eigh Positive. 

Negative Aenranm^ TOEN 

Ye=SlN(PI/4*(QE+6.0)) 

Yec=SIN(PV4*(QECA+4.0)) 

u(38)=MIN(Ye,Yec) 

UU(38)=6.0 . 

c Rule »Tne if Error .s Medium Negative and the Change in c Error er 

u(39)=MIN(Ye,Yec) 

UU(39)=4.0 

« . (ou&ot is Medium Negative and the Change in c Error tssma 

C Rule fourty u . ■ smdll Positive. 

Positive *en “^btn^ CESp) THEN 

^Ye=SIN(PI/4nQE+60)> 

Yec=SlN(PI/4*(QECA)) 

u(40)=MIN(Ye,Yec) 

UU(40)=2.0 

r t. if Error is Medium Negattve and the Change in c Error rs 

u(41)=MIN(Ye,Yec) 

UU(41)=0.0 

r if Error is Medium Negative and the Change in c Enor ts 

Ye=SIN(PV4*(QE+6.0)) 

Yec=SlN(Pl/4*(QECA-4.0)) 

u(42)=MIN(Ye,Yec) 

UU(42)=-2.0 

If f Error is Large Negative and the Change in c Error is Large 
c Rule fourty three if Error s ^g 

Negative then con—^j^L ^g 

Ye=SIN(PI/4*(QE+8.0)) 

Yec=SIN(PI/4*(QECA+8.0)) 

u(43)=MIN(Ye,Yec) 
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IRJ(43)=6.0 

Crrnr i. Large Negative and the Change m c Error is 
c Rule fourty four ^^umfon is Large Positive. 

Medium 

^ Ye=SlN(Py4nQE+8.^ 

Yec=SlN(Pl/4*(QECA+6.0)) 

u(44)=MIN(Ye,Yec) 

UU(44)=6.0 . 

if Error is Lar|e Negative and the Change in c Emor ts Stn 
C Rule fourty ^on is Large Positive. 

Negative then conm^ ^^^^^ T^N 

^ Ye-SIN(PI/4*(QE+8.0)) 

YecSm(PI/4nQECA-.4^ 

u(45)=MIN(Ye,Yec) 

UU(45)=6.0 . 

e Ruie foXl' .f Hm,r ts Large Negattve and the Change tn c Error ts Ze 
.henconmbunonisj^ge^^^^^ 

^ ^ ,e OTTVT/'DT/A*('nE+8.0)) 


NAND.CEZL) irLE'i'' 
2=SIN(PV4*(QE+8.0)) 

ec=SIN(PV4*(QECA+2)) 

rAfi^=MIN(Ye,Yec) 


Yec=oii’'v^ « ■ ' 7-^ 

u(46)=MIN(Ye,Yec) 

END™‘''^*'g° Large Negative and the Change in c Error is Small 
C Rule fotmy f Medto Positive. 

Positive =TAm™Es“ THEN 

^ Ye=SlN(Pl/4*(QE+8.0)) 

?eS\N(Py4*(QECA)) 

u(47)=MlN(Ye,Yec) 

UU(47)=4.0 

end if Negative and the Change m c Erro 

c Rule fourty e'8^V^„^^°^tibution is Small Positive. 

Medium P°“S“emTtHEN 

^ ^Ye=SlN(Py4*(QE+8.0)) 

Yec=SIN(PV4*(QECA-2.0)) 

u(48)=MlN(Ye,Yec) 

• c oriR large 

0 Rule fo^:^nfne tf Emor ts La^ge Negattve and the Change m c Err 

Yec=S IN (PI/4* (QEC A-4.0)) 
u(49)=MlN(Ye,Yec) 

UU(49)=0.0 
END IF 



c ihe NUMerator and DENomenator to zero so that only c contnbuttons 

on this pass will be considered. 

NUM=0.0 

c CalcuSliuMerator and the DENomenator of the control input c by mearts o 

the center of gravity method. 

°° ?Sm='num+u(I)*uu(I) 

DEN=DEN+u(I) 

c^Sth^Somentator to 1 .0 prevents division by zero and c does not 
DEN=1.0 

end if 

1I^UT=NUM/DEN 
IF (LINK.EQ.1) THEN 

WRITE(9, 1 6)E,CEA,QE,QECA, 

INPUT END IF 
IF (LINK.EQ.2) TffiN 

WRITE( 10,1 6)E,CE A ,QE,QEC 

A,INPUT 
end IF 

IF (LINK.EQ.3) TlffiN 

WRITE(1 1,16)E,CEA,QE,QEC 

a,input 

END IF 

c using the correct quantizied table convert the input into c a torque to be 
END IF 

IF (MEDIUM) then 

T0RQUE=INPUT* ( 1 5./6.0) 

END IF 

^^^Q™?NPUTni7./6.0) 

end IF 
return 

END 



